clear
clc

tar=[6,5];
dt=0.1;
vL=0;
vR=0;
vLo=0;
vRo=0;
vL2=0;
vR2=0;
vLo2=0;
vRo2=0;
v=0;
w=0;
do=4;
sio=0;
b=0.098;
fuV=readfis("fuzzyV.fis");
fuW=readfis("fuzzyW.fis");
%障碍物
obs_obj=readmatrix("obs_data.xlsx","Sheet","cor");
obs_num=size(obs_obj);
%

plot(tar(1),tar(2),'bO');
hold on
plot(obs_obj(:,1),obs_obj(:,2),'g*');
Xg=[];
Yg=[];
vLg=[];
vRg=[];
dgg=[];
dog=[];
dodg=[];
sigg=[];
siog=[];
siodg=[];
fogg=[];
vLog=[];
vRog=[];
vL2og=[];
vR2og=[];
dgdg=[];
sigdg=[];
aFigg=[];
for i=1:500
    [dg,sig,X,Y,car_angle,dgd,sigd,Xd,Yd,car_angle_d]=tar_po2(tar(1),tar(2),vL,vR,vLo,vRo,dt);
    [dg2,sig2,X2,Y2,car_angle2,dgd2,sigd2,Xd2,Yd2,car_angle_d2]=tar_po2_only(tar(1),tar(2),vL2,vR2,vLo2,vRo2,dt);
    dgg=[dgg,dg];
    sigg=[sigg,sig];
    dgdg=[dgdg,dgd];
    sigdg=[sigdg,sigd];
    if(dg<0.8||dg2<0.8)
        break;
    end
    plot(X,Y,'ro');
    plot(X2,Y2,'b^');
    xlim([0,7]);
    ylim([0,7]);
    xlabel("X axis");
    ylabel("Y axis");
    legend("target position","obstacle","RFNN-EKF","RFNN only");
    Xg=[Xg,X];
    Yg=[Yg,Y];
    axis equal
    [do,sio,xo,yo,dodd,siod]=obstacle_detect2(obs_obj,X,Y,Xd,Yd,car_angle,car_angle_d);
    [do2,sio2,xo2,yo2,dodd2,siod2]=obstacle_detect2(obs_obj,X2,Y2,Xd2,Yd2,car_angle2,car_angle_d2);
    dog=[dog,do];
    siog=[siog,sio];
    dodg=[dodg,dodd];
    siodg=[siodg,siod];
    if(i==28)
        j=1;
    end
    [vL,vR,vLo,vRo,aFig]=RFNN_EKF_cal_v5(tar(1),tar(2),dg,sig,X,Y,car_angle,do,sio,xo,yo,dgd,dodd,sigd,siod);
    [vL2,vR2,vLo2,vRo2]=RFNN_cal_v5(dg2,sig2,do2,sio2);
    vLg=[vLg,vL];
    vRg=[vRg,vR];
    vLog=[vLog,vLo];
    vRog=[vRog,vRo];
    vL2og=[vL2og,vLo2];
    vR2og=[vR2og,vRo2];
    aFigg=[aFigg;aFig];
end
dgmin=min(dgg);
domin=min(dog);